﻿using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using Microsoft.DirectX;
using MultiResolutioning.Data;
using MultiResolutioning.Structures;
using MultiResolutioning.Interfaces;
using MultiResolutioning.Collections;
using System;

namespace MultiResolutioning.Algorithms
{
	/// <summary>
	/// Klasa realizuje algorytm QEM.
	/// </summary>
	public class QEMSimplifier: ISimplifier
	{
		/// <summary>
		/// Rozpoczyna upraszczanie siatki.
		/// </summary>
		/// <param name="progressive">Siatka wielorozdzielcza.</param>
		/// <param name="iterations">Liczba iteracji.</param>
		/// <param name="times">Zwracany czas operacji.</param>
		public void Simplify(ProgressiveMesh progressive, int iterations, out string times)
		{
			Stopwatch timer = new Stopwatch();
			
			InitCosts(progressive);

			Heap<Vertex> heap = new Heap<Vertex>(progressive.mesh.vertices, progressive.mesh.vertices.Count);

			Vertex min;
			timer.Start();
			for (int i = 0; i < iterations; ++i)
			{
				min = heap.PeekRoot();

				Collapse(min, min.simplOperation.Target, progressive, heap);
			}
			timer.Stop();
			times = timer.Elapsed.ToString();
			progressive.mesh.faces.RemoveAll(x => x.removed);
			progressive.mesh.vertices.RemoveAll(x => x.removed);
		}

		private void InitCosts(ProgressiveMesh progressive)
		{
			// Normals and quadrics for faces.
			foreach (Face f in progressive.mesh.faces)
			{
				f.ComputeNormal();
				ComputeFaceQuadric(f);
			}

			// Quadrics for each vertex.
			foreach (Vertex v in progressive.mesh.vertices)
			{
				Matrix vQuadric = Matrix.Zero;
				foreach (Face f in v.faces)
				{
					vQuadric += (Matrix)f.additionalField;
				}

				v.additionalField = vQuadric;
			}

			// After all quadrics has been calculated we initialize contraction cost for each vertex.
			foreach (Vertex v in progressive.mesh.vertices)
			{
				CalculateCostForVertex(v);
			}
		}

		public void Collapse(Vertex v1, Vertex v2, ProgressiveMesh progressive, Heap<Vertex> heap)
		{
			if (v2 == null)
			{
				//v1.removed = true;                
				v1.simplOperation.Cost = float.MaxValue;
				heap.Update(0);
				//heap.PopRoot();
				return;
			}

			List<Face> toRemove = new List<Face>();

			// choosing faces to remove (faces which contains v1 and v2).
			foreach (Face face in v1.faces)
			{
					for (int i = 0; i < 3; ++i)
					{
						if (face.vertices[i] == v2)
						{
							toRemove.Add(face);
						}
					}
			}

			Vertex temp1 = v1.Clone();
			Vertex temp2 = v2.Clone();

			List<FaceSmall> removedFacesIds = new List<FaceSmall>(toRemove.Count);
			// removing those faces from vertices on which they are contructed.
			// removing v2 from neighbors for those faces (preventing duplicates).
			// flaging those faces as removed.
			foreach (Face f in toRemove)
			{
				removedFacesIds.Add(new FaceSmall(f));

				foreach (Vertex v in f.vertices)
				{
					v.faces.Remove(f);
					v.neighbors.Remove(v1);
					v.neighbors.Remove(v2);
				}
				f.removed = true;
			}

			// replacing v1 with v2 in v1 faces.
			for (int j = 0; j < v1.faces.Count; ++j)
			{
					for (int i = 0; i < 3; ++i)
					{
						if (v1.faces[j].vertices[i] == v1)
						{
							v1.faces[j].vertices[i] = v2;
						}
					}
			}

			// each v1 neighbor gets new neighbor v2 (if he has not it already).
			foreach (Vertex n in v1.neighbors)
			{
				if (!n.neighbors.Contains(v2))
					n.neighbors.Add(v2);

				n.neighbors.RemoveAll(x => x == v1);
			}

			// all v1 faces and neighbors goes to v2.
			v2.neighbors.AddRange(v1.neighbors);
			v2.faces.AddRange(v1.faces);

			// removing duplicates.
			v2.neighbors = v2.neighbors.Distinct().ToList();
			v2.neighbors.Remove(v1);
			v2.neighbors.Remove(v2);
			
			// removing v1 from structures.
			v1.removed = true;
			//progressive.mesh.vertices.RemoveAt(v1Index);
			heap.PopRoot();

			v2.position = v1.simplOperation.NewPosition;
			v2.additionalField = (Matrix)v1.additionalField + (Matrix)v2.additionalField;

			foreach (Vertex vertex in v2.neighbors)
			{
				vertex.neighbors = vertex.neighbors.Distinct().ToList();
				vertex.neighbors.Remove(v1);
				CalculateCostForVertex(vertex);
				heap.Update(vertex.InCollectionIndex);
			}

			CalculateCostForVertex(v2);
			heap.Update(v2.InCollectionIndex);

			v2.vertexId = progressive.mesh.GetNewVertexId();
			Vertex newVertex = v2.Clone();
			Split split = new Split(newVertex, temp1, temp2, removedFacesIds);
			progressive.AddSplit(split);
		}

		private void CalculateCostForVertex(Vertex v)
		{
			ContractionCost tempOpertionCost = new ContractionCost(float.MaxValue, null, new Vector3());
			Vector3 tempPos;
			float tempCost;

			// selecting minimal cost for each vertex (looking only for neighbors).
			foreach (Vertex n in v.neighbors)
			{
				Matrix qSum = (Matrix)v.additionalField + (Matrix)n.additionalField;

				tempPos = ComputeNewPosition(v, n, qSum);
				tempCost = ComputeCost(qSum, tempPos);

				if (tempCost < tempOpertionCost.Cost)
				{
					tempOpertionCost.Cost = tempCost;
					tempOpertionCost.NewPosition = tempPos;
					tempOpertionCost.Target = n;
				}
			}
			if (tempOpertionCost.Target == null)
				tempOpertionCost.Cost = float.MaxValue;

			v.simplOperation = tempOpertionCost;
		}

		private void ComputeFaceQuadric(Face face)
		{
			float A, B, C, D;
			float a, b, c, d, length;

			A = face.normal.X;
			B = face.normal.Y;
			C = face.normal.Z;
			D = Vector3.Dot(face.normal, face.vertices[0].position) * -1;
			length = face.normal.Length();

			a = A / length;
			b = B / length;
			c = C / length;
			d = D / length;

			Matrix quadric = new Matrix();

			quadric.M11 = a * a;
			quadric.M12 = quadric.M21 = a * b;
			quadric.M13 = quadric.M31 = a * c;
			quadric.M14 = quadric.M41 = a * d;

			quadric.M22 = b * b;
			quadric.M23 = quadric.M32 = b * c;
			quadric.M24 = quadric.M42 = b * d;

			quadric.M33 = c * c;
			quadric.M34 = quadric.M43 = c * d;

			quadric.M44 = d * d;

			face.additionalField = quadric;
		}

		private float ComputeCost(Matrix mat, Vector3 vec)
		{
			Vector4 vecTmat= new Vector4(
				vec.X * mat.M11 + vec.Y * mat.M21 + vec.Z * mat.M31 + mat.M41,
				vec.X * mat.M12 + vec.Y * mat.M22 + vec.Z * mat.M32 + mat.M42,
				vec.X * mat.M13 + vec.Y * mat.M23 + vec.Z * mat.M33 + mat.M43,
				vec.X * mat.M14 + vec.Y * mat.M24 + vec.Z * mat.M34 + mat.M44);

			return Math.Abs(vec.X * vecTmat.X + vec.Y * vecTmat.Y + vec.Z * vecTmat.Z + vecTmat.W);
		}

		private Vector3 ComputeNewPosition(Vertex v1, Vertex v2, Matrix quadricsSum)
		{
			quadricsSum.M41 = quadricsSum.M42 = quadricsSum.M43 = 0;
			quadricsSum.M44 = 1;

			// if not invertable
			Matrix temp = Matrix.Invert(quadricsSum);
			if (temp == Matrix.Zero || temp == null)
			{
				// returning position in the midle between vertices.
				return Vector3.Scale(v2.position - v1.position, 0.5f) + v1.position;
			}

			return new Vector3(temp.M14, temp.M24, temp.M34);
		}
	}
}
